#ifndef __CQUATERNION_H__
#define __CQUATERNION_H__

#include "StdDefs.h"
#include "CMathFixed.h"
#include "CVector3d.h"
#include "CMatrix3d.h"
#include "CMatrix4d.h"
#include "CMatrix4dh.h"


// CQuaternion* must maintain casting ability with (fixed*).
// CQuaternion may not have virtuals, inherited, or instantiated data.

// Portions shamelessly ripped from:
//   "quaternion.h" By Will Perone (will.perone@gmail.com)


class CQuaternion 
{
public:
	CQuaternion();
	CQuaternion( const fixed* q );
	CQuaternion( fixed i, fixed j, fixed k, fixed w ); 
	CQuaternion( fixed scalar, const CVector3d& vector );
	CQuaternion( const CMatrix4dh& m );
	CQuaternion( const CQuaternion& q );

	~CQuaternion() {}

	CQuaternion&			operator=( const CQuaternion& other );
	CQuaternion&			operator=( const fixed* q ); 

	CQuaternion				operator*( fixed scale ) const;
	CQuaternion				operator/( fixed scale ) const;
	CQuaternion				operator+( const CQuaternion &q ) const;
	CQuaternion				operator-( void ) const;

	CQuaternion&			operator*=( CQuaternion const& b );
	CQuaternion&			operator*=( fixed scale );
	CQuaternion&			operator/=( fixed scale );

	fixed&					operator[]( int idx );
	fixed const&			operator[]( int idx ) const;

	friend CQuaternion		operator*( CQuaternion const& a, CQuaternion const& b );

	//! casting to a 4x4 isomorphic matrix for right multiplication with vector
	operator CMatrix4d() const;

	//! casting to 3x3 rotation matrix
	operator CMatrix3d() const;

	//! extracting vector part of CQuaternion
	operator CVector3d() const;

	//! extracting scalar part of CQuaternion
	operator fixed() const;

	CQuaternion&			Clear();
	CQuaternion&			Identity();

	CQuaternion&			Normalize();
	CQuaternion				Normalized() const;

	CQuaternion&			Conjugate();
	CQuaternion				Conjugated() const;

	fixed					Length() const;
	fixed					LengthSquared() const;

	CQuaternion&			Invert();
	CQuaternion				Inverse() const;

	CQuaternion				log() const;
	CQuaternion				exp() const;

	//! rotates v by this CQuaternion (CQuaternion must be unit)
	CVector3d				Rotate(const CVector3d &v);

	//! rotates v by this CQuaternion (CQuaternion must be unit)
	CVector3d				Rotate( fixed i, fixed j, fixed k );

	//! dot product of two quaternions
	friend fixed			dot(const CQuaternion &q1, const CQuaternion &q2);

	//! linear CQuaternion interpolation
	friend CQuaternion		lerp(const CQuaternion &q1, const CQuaternion &q2, fixed u);

	//! spherical linear interpolation
	friend CQuaternion		slerp(const CQuaternion &q1, const CQuaternion &q2, fixed u);

	//! This version of slerp, used by squad, does not check for theta > 90.
	friend CQuaternion		slerpNoInvert(const CQuaternion &q1, const CQuaternion &q2, fixed u);

	//! spherical cubic interpolation
	friend CQuaternion		squad(const CQuaternion &q1,const CQuaternion &q2,const CQuaternion &a,const CQuaternion &b,fixed u);

	//! Shoemake-Bezier interpolation using De Castlejau algorithm
	friend CQuaternion		bezier(const CQuaternion &q1,const CQuaternion &q2,const CQuaternion &a,const CQuaternion &b,fixed u);

	//! Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
	friend CQuaternion		spline(const CQuaternion &qnm1,const CQuaternion &qn,const CQuaternion &qnp1);

	//! converts from a normalized axis - angle pair rotation to a CQuaternion
	friend CQuaternion		from_axis_angle(const CVector3d &axis, fixed angle);

	//! returns the axis and angle of this unit CQuaternion
	void					to_axis_angle(CVector3d &axis, fixed &angle) const;

	//! returns the euler angles from a rotation CQuaternion
	CVector3d				to_euler_angles() const;

	fixed					m_i;
	fixed					m_j;
	fixed					m_k;
	fixed					m_w;
};

inline
CQuaternion CQuaternion::operator*( fixed scale ) const
{
	return CQuaternion( 
		CMathFixed::Mul( m_i, scale ),
		CMathFixed::Mul( m_j, scale ),
		CMathFixed::Mul( m_k, scale ),
		CMathFixed::Mul( m_w, scale) );
}

inline
CQuaternion& CQuaternion::operator*=( fixed scale ) 
{
	m_i = CMathFixed::Mul( m_i, scale );
	m_j = CMathFixed::Mul( m_j, scale );
	m_k = CMathFixed::Mul( m_k, scale );
	m_w = CMathFixed::Mul( m_w, scale );
	return *this;
}

inline
CQuaternion CQuaternion::operator/( fixed scale ) const
{	
	return CQuaternion( 
		CMathFixed::Div( m_i, scale ),
		CMathFixed::Div( m_j, scale ),
		CMathFixed::Div( m_k, scale ),
		CMathFixed::Div( m_w, scale ) );
}

inline
CQuaternion& CQuaternion::operator/=( fixed scale )
{
	m_i = CMathFixed::Div( m_i, scale );
	m_j = CMathFixed::Div( m_j, scale );
	m_k = CMathFixed::Div( m_k, scale );
	m_w = CMathFixed::Div( m_w, scale );
	return *this;
}

inline
CQuaternion CQuaternion::operator+( const CQuaternion &q ) const
{
	return CQuaternion( m_i + q.m_i, m_j + q.m_j, m_k + q.m_k, m_w + q.m_w );
}

inline
CQuaternion CQuaternion::operator-() const
{
	return CQuaternion( -m_i, -m_j, -m_k, -m_w );
}

inline
CQuaternion::operator CMatrix4d() const
{			
	return CMatrix4d( m_w,  -m_i, -m_j, -m_k,
		              m_i,   m_w, -m_k,  m_j,
				      m_j,   m_k,  m_w, -m_i,
				      m_k,  -m_j,  m_i,  m_w );
}

inline
CQuaternion& CQuaternion::Clear()
{
	m_i = 0;
	m_j = 0;
	m_k = 0;
	m_w = 0;
	return *this;
}

inline
CQuaternion& CQuaternion::Identity()
{
	Clear();
	m_w = CMathFixed::One;
	return *this;
}

inline
fixed CQuaternion::Length() const
{
	return CMathFixed::Sqrt( dot( *this, *this ) );  
}

inline
fixed CQuaternion::LengthSquared() const
{
	return dot( *this, *this );
}

inline
CVector3d CQuaternion::Rotate(const CVector3d &v)
{
	return ( (*this) * CQuaternion( (fixed) 0, v ) ) * Conjugated();
}

inline
CVector3d CQuaternion::Rotate( fixed i, fixed j, fixed k )
{
	return ( (*this) * CQuaternion( 0, i, j, k ) ) * Conjugated();
}

inline
fixed dot(const CQuaternion &q1, const CQuaternion &q2)
{
	return CMathFixed::Mul(q1.m_w,q2.m_w) 
		 + CMathFixed::Mul(q1.m_i,q2.m_i) 
		 + CMathFixed::Mul(q1.m_j,q2.m_j) 
		 + CMathFixed::Mul(q1.m_k,q2.m_k);
}

inline
CQuaternion squad(const CQuaternion &q1,const CQuaternion &q2,const CQuaternion &a,const CQuaternion &b,fixed u)
{
	CQuaternion c = slerpNoInvert(q1,q2,u), d = slerpNoInvert(a,b,u);
	fixed uu = CMathFixed::Mul( u, CMathFixed::One - u ) << 1;
	return slerpNoInvert( c, d, uu );
}

inline
CQuaternion bezier(const CQuaternion &q1,const CQuaternion &q2,const CQuaternion &a,const CQuaternion &b,fixed u)
{
	// level 1
	CQuaternion q11= slerpNoInvert(q1,a,u),
			    q12= slerpNoInvert(a,b,u),
			    q13= slerpNoInvert(b,q2,u);		
	// level 2 and 3
	return slerpNoInvert( slerpNoInvert(q11,q12,u), slerpNoInvert(q12,q13,u), u );
}

inline
CQuaternion spline(const CQuaternion &qnm1,const CQuaternion &qn,const CQuaternion &qnp1)
{
	CQuaternion qni( qn.Conjugated() );

	CQuaternion qni_qnm1( (qni * qnm1).log() );
	CQuaternion qni_qnp1( (qni * qnp1).log() );
	
	CQuaternion tmp1 = ( qni_qnm1 + qni_qnp1 ) / CMathFixed_FloatToFixed( -4.0f );
	CQuaternion tmp2 = tmp1.exp();

	return qn * tmp2;
}

inline
CQuaternion operator*(const CQuaternion &q1, const CQuaternion &q2)
{
	CQuaternion q( q1 );
	return q*=q2;
}

inline
CQuaternion lerp(const CQuaternion &q1, const CQuaternion &q2, fixed u)
{ 
	CQuaternion q3;
	fixed d = dot( q1, q2 );

	/*	dot = cos(theta)
		if (dot < 0), q1 and q2 are more than 90 degrees apart,
		so we can invert one to reduce spinning	*/
	if (d < 0) {
		d = -d;
		q3 = -q2;
	} else {
		q3 = q2;
	}
	CQuaternion q( q1*(CMathFixed::One - u) + q3*u );
	return q.Normalized(); 
}

inline
CQuaternion from_axis_angle(const CVector3d &axis, fixed angle)
{
	return CQuaternion( CMathFixed::Cos( angle >> 1 ), axis*CMathFixed::Sin( angle >> 1 ) ); 	
}

inline
CQuaternion& CQuaternion::operator=( const fixed* q )
{ 
	ASSERT( q );
	m_i = q[0];
	m_j = q[1];
	m_k = q[2];
	m_w = q[3];
	return *this;
}


inline
fixed& CQuaternion::operator[]( int idx )
{
	ASSERT( idx < 4 );
	return ((fixed*)this)[ idx ];
}


inline
fixed const& CQuaternion::operator[]( int idx ) const
{
	ASSERT( idx < 4 );
	return ((fixed*)this)[ idx ];
}

inline
CQuaternion& CQuaternion::operator=( CQuaternion const& other ) 
{
	m_i = other.m_i;
	m_j = other.m_j;
	m_k = other.m_k;
	m_w = other.m_w;
	return *this;
}

#endif // __CQUATERNION_H__
